package edu.wpi.first.wpilibj.swarm;

import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.Utility;

public class Drivetrain implements Runnable {
	private RobotMain       robomain;
	private GyroInitializer gyroinit;
	private double          dt;
	private Gyro            gyro;
	private MechOutput      mechoutput;
	        EncoderFeedback encoderfeedback;
	private TurnPD          turnpd;
	private TransStab       transstab;
	
	private Vector          command;
	private double          rot_in;
	private boolean         fieldoriented;
	private Angle           botangle;
	private Angle           fieldcorrang;
	private Vector          transerror;
	private Vector          vel;
	
	private Vector          fieldoriented_in;
	private double          fieldoriented_in_rot;
	
	private Vector          fieldoriented_out;
	private double          fieldoriented_out_rot;
	
	private Vector          mech_out;
	private double          mech_rot_out;
	
	private static final long   loop_period = 10000; // Microseconds
	private static final double init_ang_deg = 90;
	
	Drivetrain(RobotMain robotmain) {
		gyroinit             = new GyroInitializer(1);
		gyroinit.init();
		
		dt                   = 0.0;
		robomain             = robotmain;
		mechoutput           = new MechOutput();
		botangle             = new Angle();
		botangle.setAngle(init_ang_deg);
		fieldcorrang         = new Angle();
		encoderfeedback      = new EncoderFeedback();
		turnpd               = new TurnPD(init_ang_deg, robomain);
		transstab            = new TransStab(robomain);
		transerror           = new Vector();
		vel                  = new Vector();
		
		command               = new Vector();
		rot_in                = 0.0;
		fieldoriented         = true;
		
		fieldoriented_in      = new Vector();
		fieldoriented_in_rot  = 0.0;
		
		fieldoriented_out     = new Vector();
		fieldoriented_out_rot = 0.0;
		
		mech_out              = new Vector();
		mech_rot_out          = 0.0;
	}

	public void run() {
		gyro         = gyroinit.getGyro();
		long time    = Utility.getFPGATime();
		long tgttime = Utility.getFPGATime();
		while (true) {
			dt    = ((double) (Utility.getFPGATime() - time)) / 1000000.0;
			time += dt * 1000000.0;
			computeOutput();
			mechoutput.output(mech_out, mech_rot_out);
			tgttime += loop_period;
			try {
				Thread.sleep((tgttime - Utility.getFPGATime())/1000);
			} catch (Exception ex) {}
		}
	}

	void setInput(double x, double y, double rot, boolean field_oriented) {
		synchronized(this) {
			command.setX(x);
			command.setY(y);
			rot_in          = rot;
			fieldoriented   = field_oriented;
		}
	}
	
	private double nonlinearScale(double in) {
		return in * (0.7 * (1 - Math.abs(in)) + Math.abs(in));
	}
	
	private void fieldoriented() {
		fieldcorrang.setAngle(-botangle.getAngle());
		fieldcorrang.rotate(fieldoriented_in, fieldoriented_out);
		fieldoriented_out.setX(nonlinearScale(fieldoriented_out.getX()));
		fieldoriented_out_rot = fieldoriented_in_rot;
	}
	
	private void rotStab() {
		mech_rot_out = turnpd.control(encoderfeedback.getAngularRate(), botangle,
		                              fieldoriented_out_rot);
		//mech_rot_out = fieldoriented_out_rot;
	}
	
	private void transStab() {
		encoderfeedback.updateErr(transerror);
		encoderfeedback.updateVel(vel);
		transstab.control(transerror, vel, fieldoriented_out,
		                             mech_out);
	}
	
	void resetGyro() {
		gyro.reset();
		botangle.setAngle(Math.toRadians(init_ang_deg));
		turnpd.resetTgt(botangle);
	}

	private void computeOutput() {
		double temp = botangle.getAngle();
		botangle.setAngle(Math.toRadians(gyro.getAngle() + init_ang_deg));
		//System.out.println(botangle.getAngle());
		synchronized(this) {
			fieldoriented_in.set(command);
			fieldoriented_in_rot = rot_in;
		}
		
		encoderfeedback.update();
		if (fieldoriented) {
			fieldoriented();
			transStab();
			rotStab();
		} else {
			transstab.reset();
			turnpd.reset();
			mech_out     = fieldoriented_in;
			mech_rot_out = fieldoriented_in_rot;
		}
	}
}